#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>  // 修改为压缩图像消息
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
// #include "image_transport/image_transport.hpp"  // 如果不使用 image_transport，可以注释掉

using namespace std::chrono_literals;

class USBCameraPublisher : public rclcpp::Node
{
public:
    USBCameraPublisher() : Node("usb_camera_publisher")
    {
        // 创建一个发布者，发布到 "camera/image_raw/compressed" 话题
        // 选择合适的队列大小，例如10
        publisher_ = this->create_publisher<sensor_msgs::msg::CompressedImage>("camera/image_raw/compressed", 30);
        
        // 定时器，每33毫秒触发一次回调（约30 FPS）
        timer_ = this->create_wall_timer(
            33ms, std::bind(&USBCameraPublisher::timer_callback, this));

        // 打开摄像头（0表示默认摄像头）
        cap_ = cv::VideoCapture("/dev/video0");  // 通常用0代表默认摄像头
        if (!cap_.isOpened()) {
            RCLCPP_ERROR(this->get_logger(), "无法打开摄像头！");
        } else {
            cap_.set(cv::CAP_PROP_FPS, 30);
            cap_.set(cv::CAP_PROP_FRAME_WIDTH, 640);
            cap_.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
            RCLCPP_INFO(this->get_logger(), "摄像头已成功打开。");
        }
    }

private:
    void timer_callback()
    {
        cv::Mat frame;
        cap_ >> frame;  // 从摄像头捕获一帧
        if (frame.empty()) {
            RCLCPP_WARN(this->get_logger(), "没有捕获到图像！");
            return;
        }

        // 压缩图像，例如使用 JPEG 压缩
        std::vector<uchar> encoded_image;
        std::vector<int> compression_params = {cv::IMWRITE_JPEG_QUALITY, 90};  // 压缩质量可调整
        bool success = cv::imencode(".jpg", frame, encoded_image, compression_params);
        if (!success) {
            RCLCPP_WARN(this->get_logger(), "图像压缩失败！");
            return;
        }

        // 创建并填充 CompressedImage 消息
        auto msg = sensor_msgs::msg::CompressedImage();
        msg.header = std_msgs::msg::Header();  // 您可以根据需要设置时间戳和帧 ID
        msg.header.stamp = this->get_clock()->now();
        msg.header.frame_id = "camera_frame";  // 根据您的应用设置
        msg.format = "jpeg";  // 或 "png" 根据使用的编码格式
        msg.data = std::move(encoded_image);  // 移动数据以提高效率

        // 发布压缩图像
        publisher_->publish(msg);
    }

    rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    cv::VideoCapture cap_;
};

int main(int argc, char **argv)
{
    // 初始化ROS 2
    rclcpp::init(argc, argv);

    // 创建节点
    auto node = std::make_shared<USBCameraPublisher>();

    // 创建多线程执行器
    rclcpp::executors::MultiThreadedExecutor executor;

    // 将节点添加到执行器
    executor.add_node(node);

    // 运行执行器（多线程）
    executor.spin();

    // 关闭ROS 2
    rclcpp::shutdown();

    return 0;
}
